#ifndef _LIDAR_H_
#define _LIDAR_H_

// Function headers
void initializeMaps();
void filterMap(double, int);
void constructOccupancyGrid();
void expandOccupancyGrid();

// Control table addresses
#define GOAL_POSITION_L		30
#define MOVING_SPEED_L 		32
#define TORQUE_LIMIT_L		34
#define PRESENT_POSITION_L	36
#define MOVING			46

// Defualt settings
#define DEFAULT_BAUD		1 // MBPS 
#define DEFAULT_ID		1
#define DEFAULT_TORQUE		1023

// Constants
#define ROBOT_WIDTH 		100
#define PTS_PER_TEN_CM 		6
#define OBSTACLE_GROWTH 	(ROBOT_WIDTH / 10 / 2) 
#define HALF_PI			(3.14159266 / 2)

// Positions to reach
extern const int posHigh;
extern const int posLow;
extern const int speedHigh; // RPM
extern const int speedLow; // RPM

// 2D array to store 3D data
extern const int posDiff;
extern const int numScans;
extern const int occHeight;
extern const int occWidth;
extern double **map3d;
extern int **occupancy;

#endif
